视觉SLAM求解位姿
把之前学的视觉SLAM的求解位姿总结一下
Bundle Adjustment
定义
从特征点反射出来的几束光线（bundles of light rays）
在姿态和路标点被最优调整（adjustment）之后
最后收束到光心的这个过程，称为Bundle Adjustment，简称BA，也被译做光束平差法
流程
个人觉得涉及到像素点和路标点的映射的最小二乘求解，都可以算是BA的范畴
所以这里具体讲述非线性最小二乘求解的原理，具体映射关系后续分情况讲述
对于任意函数f(x)，求解x使得21∣∣f(x)∣∣22最小，LM方法求解步骤：
①给定初始值x0，以及信赖区域半径μ
②对于第k次迭代，求解Δxk，D为约束区域因子（可取I）
(JT(xk)J(xk)+λDTD)Δxk=−JT(xk)f(xk)
③计算ρ=J(x)Δxf(x+Δx)−f(x)，μ={2μ,0.5μ,ρ>43ρ<41
④若ρ大于某阈值，认为近似可行，xk+1=xk+Δxk
⑤判断算法是否收敛，不收敛则返回2，收敛则结束
使用ceres库可以很方便地求解，我们只需要定义对应的f(x)和雅可比矩阵J即可
常见求解方法
对极几何（2D_2D）
输入：需要一帧的RGB图，需要另一帧的RGB图
输出：可以获得两帧之间的位姿变换，以及两帧图像上的点对应的路标点
流程：
①对两张RGB图特征点提取、计算描述子、特征匹配（匹配后筛选）
②有对极约束：p2T(K−1)Tt^RK−1p1=0，假设E=t^R，八点法或最小二乘求E，
③对结果E后进行SVD奇异值分解，E=U∑VT，求出四组解，过滤解得R,t
④获得R,t后三角化求解深度，进而解得路标点P
P3P（3D_2D）
输入：需要一帧的RGBD图，需要另一帧的RGB图
输出：可以获得两帧之间的位姿变换，以及两帧图像上的点对应的路标点
流程：
①对两张RGB图特征点提取、计算描述子、特征匹配（匹配后筛选）
②对RGBD图的一帧计算路标点
③对一帧的3D点以及对应的另外一帧的2D点，通过几何求解或者通过BA求解位姿
BA求解：
①投影关系为：su=Kexp(ξ^)P，定义ei=ui−si1Kexp(ξ^)Pi，
故问题为求解S=argmin(21∑i=1n∣∣ui−si1Kexp(ξ^)Pi∣∣22)
②求雅可比，∂δξ∂e=∂P′∂e∂δξ∂P′，P′=exp(ξ^)P
∂P′∂e，像素点对路标点求导，展开各自求导即可；∂δξ∂P′，采用李代数扰动模型求解即可
∂δξ∂e=∂P′∂e∂δξ∂P′=−[Z′fx00Z′fy−Z′2fxX′−Z′2fyY′−Z′2fxX′Y′−fy−Z′2fyY′2fx+Z′2fxX′2Z′2fyX′Y′−Z′fxY′Z′fyX′]
ICP（3D_3D）
输入：需要一帧的RGBD图，需要另一帧的RGBD图
输出：可以获得两帧之间的位姿变换
流程：
①对两张RGB图特征点提取、计算描述子、特征匹配（匹配后筛选）
②对RGBD图的两帧计算路标点
③对两帧的对应的3D点，通过矩阵求解或者通过BA求解位姿
BA求解流程：
①投影关系为：P=RP′+t，定义ei=pi−(Rpi′+t)
故问题为求解minR,tS=21∑i=1n∣∣pi−(Rpi′+t)∣∣22
②求雅可比，∂δξ∂e=∂P′∂e∂δξ∂P′，P′=exp(ξ^)P
∂P′∂e，就是−I；∂δξ∂P′，采用李代数扰动模型求解即可
∂δξ∂e=∂P′∂e∂δξ∂P′=−I∂δξ∂P′=⎣⎢⎡−1000−1000−10Z′−Y′−Z′0X′Y′−X′0⎦⎥⎤
直接法
输入：需要一帧的RGBD图，需要另一帧的RGB图
①选取求解点，稀疏则用特征点即可，半稠密可以选择梯度较大的点，稠密则为整幅图像的点
②对RGBD图的一帧计算求解点对应的路标点
③对路标点，以及两幅图像，通过BA求解位姿
BA求解流程：
假设空间中一点P，在第一帧的投影点为p1，在第二帧的投影点为p2
假设第一帧到第二帧的初始估计为R,t
则有p1=Z11KP,p2=Z21Kexp(ξ^)P
定义光度误差e=I1(p1)−I2(p2)
故有最小二乘问题minξJ=∑i=1NeiTei
根据扰动模型化简得
e(ξ⊕δξ)==e(ξ)−∂u∂I2∂q∂u∂δξ∂qδξ
其中q=δξ^exp(ξ^)P,u=Z21Kq
其中∂u∂I2 表示坐标在图像的梯度，∂q∂u 表示像素点对投影点的导数，∂δξ∂q 表示投影点对位姿的导数，雅可比计算如下：
J=−∂u∂I2∂δξ∂u=−∂u∂I2[Zfx00Zfy−Z2fxX−Z2fyY−Z2fxXY−fy−Z2fyY2fx+Z2fxX2Z2fyXY−ZfxYZfyX]
